
#include "planner_pkg/astar.hpp"
#include "queue"
#include "unordered_map"

namespace planner_pkg
{

    /**
     * @brief  Constructs the planner
     * @param nx The x size of the map
     * @param ny The y size of the map
     */
    Astar::Astar(int nx, int ny)
    {
        setMapSize(nx, ny);
    }
    // 析构函数
    Astar::~Astar()
    {
        if (costmap_)
        {
            delete[] costmap_;
        }
    }

    void Astar::setMapSize(int nx_size, int ny_size) // 223,163
    {
        this->nx = nx_size;
        this->ny = ny_size;
        this->ns = nx * ny;
        // RCLCPP_INFO(rclcpp::get_logger("astar"), "map size: %d, %d", nx, ny);
        costmap_ = new unsigned char[ns];
    }

    void Astar::setCostmap(unsigned char *cmap)
    {
        // 一维数组，索引 int index = y * width + x;
        costmap_ = cmap;
    }

    void
    Astar::setGoal(int *g)
    {
        goal[0] = g[0];
        goal[1] = g[1];
        RCLCPP_DEBUG(rclcpp::get_logger("astar"), "goal: %d,%d", goal[0], goal[1]);
        RCLCPP_INFO(rclcpp::get_logger("astar"), "goal cost: %d", costmap_[g[0] + g[1] * nx]);
    }

    void
    Astar::setStart(int *g)
    {
        start[0] = g[0];
        start[1] = g[1];
        // RCLCPP_DEBUG(rclcpp::get_logger("astar"), "start: %d,%d", start[0], start[1]);
    }
    // A* 算法
    std::vector<std::shared_ptr<PointNode>> Astar::searchPath()
    {
        std::priority_queue<std::shared_ptr<PointNode>, std::vector<std::shared_ptr<PointNode>>, CompareNode> openList; // 创建优先队列
        std::unordered_map<int, std::shared_ptr<PointNode>> openListMap;
        std::unordered_map<int, std::shared_ptr<PointNode>> closedListMap;

        auto startNode = std::make_shared<PointNode>(start[0], start[1], nullptr, 0, 0);
        openList.push(startNode); // 将起始节点加入待评估集合
        openListMap[start[0] * ny + start[1]] = startNode;

        while (!openList.empty())
        {
            auto currentNode = openList.top();
            openList.pop();
            openListMap.erase(currentNode->x * ny + currentNode->y); // 移除节点

            // 到达目标点，则返回路径
            if (fabs(currentNode->x - goal[0]) < xy_goal_tolerance_ && fabs(currentNode->y - goal[1]) < xy_goal_tolerance_)
            {
                return getPath(currentNode);
            }

            closedListMap[currentNode->x * ny + currentNode->y] = currentNode; // 将当前节点加入已评估集合
            // 遍历当前节点的相邻节点
            for (int dx = -1; dx <= 1; dx++)
            {
                for (int dy = -1; dy <= 1; dy++)
                {
                    if (dx == 0 && dy == 0)
                    {
                        continue;
                    }
                    int neighborX = currentNode->x + dx;
                    int neighborY = currentNode->y + dy;
                    // 超出坐标系
                    if (neighborX >= nx || neighborY >= ny || neighborX < 0 || neighborY < 0)
                    {
                        continue;
                    }
                    if (costObs(neighborX, neighborY) >= COST_OBS_ROS) // 障碍物或在膨胀半径内
                    {
                        continue;
                    }

                    int neighborKey = neighborX * ny + neighborY;
                    if (closedListMap.find(neighborKey) != closedListMap.end())
                    {
                        continue; // 跳过已经在closedList中的节点
                    }
                    int gCost = currentNode->g + std::sqrt(dx * dx + dy * dy);
                    int hCost = heuristic(neighborX, neighborY);
                    std::shared_ptr<PointNode> neighborNode = openListMap[neighborKey];
                    // 如果邻居节点不在待评估集合中，则将其加入
                    if (neighborNode == nullptr)
                    {
                        neighborNode = std::make_shared<PointNode>(neighborX, neighborY, currentNode, gCost, hCost);
                        openList.push(neighborNode);
                        openListMap[neighborKey] = neighborNode;
                    }
                    // 如果邻居节点已经在待评估集合中，检查是否找到了更优路径
                    else if (gCost < neighborNode->g)
                    {
                        neighborNode->g = gCost;
                        neighborNode->f = neighborNode->g + neighborNode->h;
                        neighborNode->parent = currentNode;
                    }
                }
            }
        }

        return {}; // Return an empty path if no path is found
    }
    // 重构路径函数，返回从起始节点到目标节点的路径
    std::vector<std::shared_ptr<PointNode>> Astar::getPath(std::shared_ptr<PointNode> current)
    {
        std::vector<std::shared_ptr<PointNode>> total_path;
        while (current != nullptr)
        {
            total_path.push_back(current);
            current = current->parent;
        }
        std::reverse(total_path.begin(), total_path.end());
        return total_path;
    }
    int Astar::heuristic(int nodeX, int nodeY)
    {
        // return std::abs(nodeX - goal[0]) + std::abs(nodeY - goal[1]);
        // 欧几里得距离
        double dist = std::sqrt(std::pow(nodeX - goal[0], 2) + std::pow(nodeY - goal[1], 2));
        return dist;
    }

    int Astar::costObs(int x, int y)
    {
        int weight = 3;
        for (int dx = -weight; dx < weight; dx++)
        {
            for (int dy = -weight; dy < weight; dy++)
            {
                int node_x = x + dx;
                int node_y = y + dy;
                // 超出坐标系
                if (node_x >= nx || node_y >= ny || node_x < 0 || node_y < 0)
                {
                    continue;
                }
                if (costmap_[node_y * nx + node_x] >= COST_OBS_ROS) // 障碍物或在膨胀半径内
                {
                    return costmap_[node_y * nx + node_x];
                }
            }
        }
        return 0;
    }
} // namespace planner_pkg